#include "nds.h"
#include <nds/arm9/console.h> //basic print funcionality
#include <stdio.h>

#include "micro.h"

// XSVF file that implements GPIO
#include "gpio_xsvf.h"

// define new hardware registers
#define REG_XA (GBA_BUS[0])
#define REG_XB (GBA_BUS[4])
#define REG_DDRA (GBA_BUS[2])
#define REG_DDRB (GBA_BUS[8])


// callback used by XSVF player
static int xsvf_data_ptr = 0;
void readByte(unsigned char *data) {
        *data = gpio_xsvf[xsvf_data_ptr++];
        if((xsvf_data_ptr & 1023) == 0)
            iprintf(".");
}


//---------------------------------------------------------------------------------
int main(void) {
//---------------------------------------------------------------------------------
//
        irqInit();
        irqSet(IRQ_VBLANK, 0);


	touchPosition touchXY;

        //////////////////
        // setup basic console
	videoSetMode(0);	//not using the main screen
	videoSetModeSub(MODE_0_2D | DISPLAY_BG0_ACTIVE);	//sub bg 0 will be used to print text
	vramSetBankC(VRAM_C_SUB_BG);

	SUB_BG0_CR = BG_MAP_BASE(31);

	BG_PALETTE_SUB[255] = RGB15(31,31,31);	//by default font will be rendered with color 255

	//consoleInit() is a lot more flexible but this gets you up and running quick
	consoleInitDefault((u16*)SCREEN_BASE_BLOCK_SUB(31), (u16*)CHAR_BASE_BLOCK_SUB(0), 16);
        /////////////////////

        // give ARM9 control over the GBA slot
        sysSetBusOwners(BUS_OWNER_ARM9, BUS_OWNER_ARM9);

	iprintf("CPLD GPIO test! \n");

        // attempt to flash CPLD
        int res = xsvfExecute();
        iprintf("%d\n", res);
        if(res == 0) {
            iprintf("Success! Press START to continue.");
        }
        else {
            iprintf("Failure flashing CPLD!");
        }
        scanKeys();
        while(!(keysDown() & KEY_START)) scanKeys();

        iprintf(CON_CLS());


        // initialize values
        REG_DDRA = 0xff;
        REG_XA = 0x00;

        u8 pin = 0x00;
        u8 ddr = 0x00;
        REG_XA = pin;
        REG_DDRA = ddr;

        int keys=0,keys0=0;

	while(1) {
                scanKeys();
		touchXY=touchReadXY();
		iprintf("\x1b[0;0HCPLD GPIO test!\n");
		iprintf("\x1b[2;0HTouch x = %04X, %04X\n", touchXY.x, touchXY.px);
		iprintf("Touch y = %04X, %04X\n", touchXY.y, touchXY.py);

		iprintf("\x1b[4;0HNote: If pins are unconnected\n(floating), then their value\nwhen read is undefined.\n");
                iprintf("\nThis tests the XA port (on the\nleft when cart is used in NDS)");

                // retrieve current values from cart
                u8 pin1 = REG_XA;
                u8 ddr1 = REG_DDRA;

                // print out current values from cart, as well as our stored values
                iprintf("\x1b[12;0H Data on pins: 0x%02x / 0x%02x ", pin, pin1);
                iprintf("\x1b[16;0H Data Direction: 0x%02x / 0x%02x ", ddr, ddr1);

                iprintf("\x1b[20;0H1st line=your set vale\n2nd line=value sent by cart");

                // print out each bit of the registers
                int x;
                for(x=0; x<6; x++) {
                    iprintf("\x1b[%d;%dH%d ", 13, x*4 +2, (pin & BIT(x))?1:0);
                    iprintf("\x1b[%d;%dH%d ", 14, x*4 +2, (pin1 & BIT(x))?1:0);

                    iprintf("\x1b[%d;%dH%s ", 17, x*4 +2, (ddr & BIT(x))?"Out":"In");
                    iprintf("\x1b[%d;%dH%s ", 18, x*4 +2, (ddr1 & BIT(x))?"Out":"In");
                }
                keys0=keys;
                keys=keysHeld();

                // if you tap on of the bits, change the value
                if((keys & KEY_TOUCH) && !(keys0 & KEY_TOUCH)) {
                    int x=touchXY.px / 8;
                    int y=touchXY.py / 8;

// simple define to toggle a bit
#define tbit(c, b) c ^= BIT(b)

                    if(y >= 12 && y <= 14) {
                        tbit(pin, x/4); // update our stored value
                        REG_XA = pin; // send the value to the cart
                    }
                    if(y >= 16 && y <= 18) {
                        tbit(ddr, x/4);
                        REG_DDRA = ddr;
                    }
                }

	}

	return 0;
}
